#include "radar_processor.h"

// 处理雷达数据
std::vector<TargetInfo> RadarProcessor::process(const std::vector<std::vector<Pulse>> &antenna_data)
{
    std::vector<DetectionCell> all_detections;

    // 对每个天线通道进行处理
    for (int ant_idx = 0; ant_idx < params.num_antennas; ++ant_idx)
    {
        // 1. MTI/MTD处理
        auto mti_data = mti_mtd_processor->processMTI(antenna_data[ant_idx]);
        auto mtd_matrix = mti_mtd_processor->processMTD(mti_data);

        // 2. 计算功率谱
        std::vector<std::vector<float>> power_spectrum(
            params.num_samples,
            std::vector<float>(params.num_pulses - 1, 0.0f));

        for (int i = 0; i < params.num_samples; ++i)
        {
            for (int j = 0; j < params.num_pulses - 1; ++j)
            {
                power_spectrum[i][j] = SignalProcessor::computePowerDB(mtd_matrix[i][j]);
            }
        }

        // 3. CFAR检测
        for (int i = 0; i < params.num_samples; ++i)
        {
            auto detections = cfar_detector->detect(power_spectrum[i]);
            for (int doppler_bin : detections)
            {
                DetectionCell cell;
                cell.range_bin = i;
                cell.doppler_bin = doppler_bin;
                cell.angle_bin = ant_idx; // 暂时存储天线索引
                cell.power = power_spectrum[i][doppler_bin];
                all_detections.push_back(cell);
            }
        }
    }

    // 4. 角度估计
    estimateAngles(all_detections, antenna_data);

    // 5. 点迹凝聚
    return plot_aggregator->aggregate(all_detections, params);
}


// 角度估计
void RadarProcessor::estimateAngles(std::vector<DetectionCell> &detections,
                                    const std::vector<std::vector<Pulse>> &antenna_data)
{
    // 生成角度搜索范围
    std::vector<float> angles(181);
    for (int i = 0; i <= 180; ++i)
    {
        angles[i] = i - 90.0f; // -90° 到 +90°
    }

    for (auto &det : detections)
    {
        // 提取同一距离-多普勒单元的所有天线数据
        std::vector<Complex> ant_samples(params.num_antennas);
        for (int ant_idx = 0; ant_idx < params.num_antennas; ++ant_idx)
        {
            // 注意：这里需要根据MTD处理后的数据索引来获取
            // 简化处理，实际应根据具体数据结构调整
            ant_samples[ant_idx] = antenna_data[ant_idx][det.doppler_bin][det.range_bin];
        }

        // 估计角度
        det.angle_bin = static_cast<int>(angle_estimator->estimateAngle(ant_samples, angles) + 90.0f);
    }
}